#include <stdlib.h>
#include "Hexapod.h"
#include "rtthread.h"


void hexapodInit(Hexapod* hexapod) {

	for (uint8_t i = 0; i < LEG_CNT; i++) {
		legInit(i, &hexapod->leg[i]);
	}
	for (uint8_t i = 0; i < LEG_CNT; i++) {
		hexapod->gaitG.firstRun = false;
		hexapod->gaitG.startP[i].x = hexapod->leg[i].bodyPos.x;
		hexapod->gaitG.startP[i].y = hexapod->leg[i].bodyPos.y;
		hexapod->gaitG.startP[i].z = hexapod->leg[i].bodyPos.z;
	}
		hexapod->gaitG.gaitHeight = 80;
}


void hexapodProcess(Hexapod* hexapod) {	
	// 周期 20
	const float P_PI = 3.1415926 * 2;
	const uint8_t period = 20;
	//LegPos legPos[20] = {0};
	Point3D endPos[6] = {0};
	// pos, rotate, direction, radiusX, radiusY, radiusZ
//	create_tripod_steps(legPos, 0, false, 0, 10, 8);
//	for(uint8_t i = 0; i < period; i++) {
		Point3D tmp = {0, 40, -30};
		for(uint8_t i = 0; i < 6; i++) {
			endPos[i] = opPoint3dPlus(&hexapod->leg[i].bodyPos, &tmp);
			hexapod->gaitG.startP[i].x = hexapod->leg[i].bodyPos.x;
			hexapod->gaitG.startP[i].y = hexapod->leg[i].bodyPos.y;
			hexapod->gaitG.startP[i].z = hexapod->leg[i].bodyPos.z;
			hexapod->gaitG.endP[i].x = endPos[i].x;
			hexapod->gaitG.endP[i].y = endPos[i].y;
			hexapod->gaitG.endP[i].z = endPos[i].z;
		}
		
		uint8_t total = 8;
		float tim = 1.0f / total;
		uint8_t cnt = 0;
		Point3D pos ={0};
		for(float phase = 0; phase < 1.0f && cnt < total; cnt++) {
			phase += tim;
			for(uint8_t i = 0; i < LEG_CNT; i++) {
				hexapod->gaitG.phase[i] = phase;
				gaitGetFootPos(&hexapod->gaitG, i,&pos);
				setBodyTip(&pos, &hexapod->leg[i]);
				rt_kprintf("leg[%d] posxyz is %2f,%2f,%2f\n",i,pos.x,pos.y,pos.z);
			}
			rt_kprintf("--------------------------------------------------\n");
			rt_thread_delay(100);//send new pos every 250ms
		}
		
		hexapod->gaitG.gaitHeight = 0;
		tmp.x = 0;
		tmp.y = -40;
		cnt = 0;
		
		for( uint8_t i = 0; i < 6; i++) {
			endPos[i] = opPoint3dPlus(&hexapod->leg[i].bodyPos, &tmp);
			hexapod->gaitG.startP[i].x = hexapod->leg[i].bodyPos.x;
			hexapod->gaitG.startP[i].y = hexapod->leg[i].bodyPos.y;
			hexapod->gaitG.startP[i].z = hexapod->leg[i].bodyPos.z;
			hexapod->gaitG.endP[i].x = endPos[i].x;
			hexapod->gaitG.endP[i].y = endPos[i].y;
			hexapod->gaitG.endP[i].z = endPos[i].z;
		}		
		for(float phase = 0; phase < 1.0f && cnt < total; cnt++) {
			phase += tim;
			for(uint8_t i = 0; i < LEG_CNT; i++) {
				hexapod->gaitG.phase[i] = phase;
				gaitGetFootPos(&hexapod->gaitG, i,&pos);
				setBodyTip(&pos, &hexapod->leg[i]);
				rt_kprintf("leg[%d] posxyz is %2f,%2f,%2f\n",i,pos.x,pos.y,pos.z);
			}
			rt_kprintf("--------------------------------------------------\n");
			rt_thread_delay(100);//send new pos every 250ms		
		}

}

void setCalibration() {

}

void saveCalibration() {

}

void loadCalibration() {

}

void hexapod_test() {
	Hexapod hexpod = {0};
	hexapodInit(&hexpod);
	rt_thread_delay(2000);
	hexapodProcess(&hexpod);
}